(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    119826,       3341]
NotebookOptionsPosition[    118360,       3291]
NotebookOutlinePosition[    118820,       3309]
CellTagsIndexPosition[    118777,       3306]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "2", "d", " ", "kneed", " ", "biped", " ", "without", " ", "torso"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"with", " ", "all", " ", "output", " ", "functions"}], "*)"}], 
  "\[IndentingNewLine]", "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{"SetDirectory", "[", 
    RowBox[{"NotebookDirectory", "[", "]"}], "]"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\[IndentingNewLine]", 
   RowBox[{"<<", "Linearize.m"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527341306783598*^9, 
   3.5273413258006372`*^9}, {3.5273418861526527`*^9, 3.527341944516119*^9}, 
   3.527342084297084*^9, {3.527342119801113*^9, 3.527342122074991*^9}, 
   3.5273422671086893`*^9, 3.5273434152081633`*^9, {3.527343919855567*^9, 
   3.527343961917466*^9}, {3.529328352989338*^9, 3.529328362792905*^9}, {
   3.5293284284450502`*^9, 3.529328446480061*^9}, 3.52936031599969*^9, 
   3.529360702430851*^9, 3.529361713686895*^9, {3.529429715381692*^9, 
   3.529429716870432*^9}}],

Cell[BoxData[
 FormBox["\<\"/home/shu/workspace/Research/HM_Output_NTorso/model\"\>", 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.529429717620055*^9, 3.529429898942766*^9, 
  3.5294300279855347`*^9, 3.52958133051681*^9, 3.52958490708173*^9, 
  3.529586468860819*^9, 3.529589177307845*^9, 3.529616550313779*^9, 
  3.529620616213159*^9, 3.52962217066285*^9, 3.529711935400263*^9, 
  3.529755354794169*^9, 3.529762988929859*^9, 3.529766676540263*^9, 
  3.529768063216579*^9, 3.529768528367755*^9, 3.529795037613229*^9, 
  3.530716100836903*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"SetDirectory", "[", 
      RowBox[{
       RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}],
       "]"}], ";"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"choose", " ", "the", " ", "output", " ", "combination"}], "*)"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LineNumber", " ", "=", " ", "1"}], ";", "\n", 
    RowBox[{"IndexAllPre", " ", "=", " ", 
     RowBox[{"Import", "[", 
      RowBox[{"\"\<data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], ";",
     "\[IndentingNewLine]", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], "*)"}], 
    "\[IndentingNewLine]", 
    RowBox[{"IndexAll", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], "\n", 
    "\[IndentingNewLine]", 
    RowBox[{"HipPosIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"NSslopeIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"TorsoIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}], ";"}], 
   "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Lc", "\[Rule]", 
        FractionBox["3806", "10000"]}], ",", 
       RowBox[{"Lt", "\[Rule]", 
        FractionBox["4522", "10000"]}], ",", 
       RowBox[{"LT", "\[Rule]", 
        FractionBox["3273", "10000"]}], ",", 
       RowBox[{"mh", "\[Rule]", 
        FractionBox["471", "10"]}], ",", 
       RowBox[{"mt", "\[Rule]", 
        FractionBox["694", "100"]}], ",", 
       RowBox[{"mc", "\[Rule]", 
        FractionBox["323", "100"]}], ",", 
       RowBox[{"mf", "\[Rule]", 
        FractionBox["1006", "1000"]}], ",", 
       RowBox[{"g", "\[Rule]", 
        FractionBox["981", "100"]}]}], "}"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"mean", " ", "human", " ", "model"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"ndof", " ", "=", " ", "4"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"mm", "=", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "mf", ",", "mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc", ",", 
        "mf"}], "}"}], "/.", "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"statesubs", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", "i", "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "i"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", 
           RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"mm", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "mf", ",", "mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc", ",", 
         "mf"}], "}"}], "/.", "constsubs"}]}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"p0", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", " ", "0"}], ",", 
       
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", " ", "0"}], ",", 
       
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", " ", 
        "0"}], ",", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", " ", 
        "0"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], "\n", " ", 
   RowBox[{"(*", 
    RowBox[{
    "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
    "*)"}], 
   RowBox[{"(*", 
    RowBox[{"positive", " ", "z", " ", "direction"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Xi]", "q4"], "=", 
      RowBox[{"RevoluteTwist", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}]}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"1", "-", 
            RowBox[{"433", "/", "1000"}]}], ")"}], "Lc"}]}], "}"}]}], "]"}]}],
     ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"1", "-", 
             RowBox[{"433", "/", "1000"}]}], ")"}], "Lt"}]}]}], "}"}]}], 
      "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"1", "-", 
             RowBox[{"433", "/", "1000"}]}], ")"}], "Lt"}]}]}], "}"}]}], 
      "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "6"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"1", "-", 
            RowBox[{"433", "/", "1000"}]}], ")"}], "Lc"}]}], "}"}]}], "]"}]}],
     ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "7"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}],
     "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "6"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "6"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "7"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "7"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{"(*", "aniplot", "*)"}], "\n", 
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
        "]"}], "/.", "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            RowBox[{
             SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
            "\[LeftDoubleBracket]", 
            RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"2", "*", 
              RowBox[{"(", 
               RowBox[{"mf", "+", "mc", "+", "mt"}], ")"}]}], "+", "mh"}], 
            ")"}]}]}]}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}], 
   "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "kinetic", " ", "energy", " ", "and", " ", 
     "manipulator", " ", "inertia", " ", "matrix"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"For", "[", 
     RowBox[{
      RowBox[{"i", "=", "1"}], ",", 
      RowBox[{"i", "\[LessEqual]", 
       RowBox[{"Length", "[", "mm", "]"}]}], ",", 
      RowBox[{"i", "++"}], ",", 
      RowBox[{
       SubscriptBox["v", "i"], "=", 
       RowBox[{"Simplify", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[PartialD]", 
           RowBox[{"{", 
            RowBox[{"t", ",", "1"}], "}"}]], 
          RowBox[{"RigidPosition", "[", 
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], "]"}]}], "/.", 
         "\[InvisibleSpace]", "constsubs"}], "]"}]}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"T", "=", 
     RowBox[{
      FractionBox["1", "2"], " ", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{
         SubscriptBox["v", "i"], ".", 
         SubscriptBox["v", "i"]}]}]}]}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "dqe", "]"}], ",", "2"}], "}"}]], "T"}], "/.",
        "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
         "\[RightDoubleBracket]"}]}], "]"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "\[Rule]", ".923"}]}], "}"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"h", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
        "\[InvisibleSpace]", "p0"}], "/.", "constsubs"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527341306783598*^9, 
   3.5273413258006372`*^9}, {3.5273418861526527`*^9, 3.527341944516119*^9}, 
   3.527342084297084*^9, {3.527342119801113*^9, 3.527342122074991*^9}, 
   3.5273422671086893`*^9, 3.5273434152081633`*^9, {3.527343919855567*^9, 
   3.527343961917466*^9}, {3.529328352989338*^9, 3.529328362792905*^9}, {
   3.5293284284450502`*^9, 3.529328446480061*^9}, 3.52936031599969*^9, 
   3.529360702430851*^9, 3.529361713686895*^9, 3.529429715381692*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"(*", 
    RowBox[{"torso", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"theta4", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
      RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], "\n", 
    RowBox[{"TorsoHipAngle", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], "\n", 
    RowBox[{"nstorso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
      RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\n", 
    RowBox[{"LinearNStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
      RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
    RowBox[{"storso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";", "\n", 
    RowBox[{"(*", 
     RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], "\n", 
    RowBox[{"LinearStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"storso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{
    "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
    "*)"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"non", "-", 
    RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"linearized", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"HipAngle", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"hip", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"Hip", " ", "Position"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"LHipPos", " ", "=", " ", 
      RowBox[{
       RowBox[{
        RowBox[{"-", "Lc"}], " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
       RowBox[{"Lt", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}]}]}], 
     ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"Linearized", " ", "Hip", " ", "Position"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      RowBox[{"-", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}], ";"}], 
    "*)"}], 
   RowBox[{"(*", 
    RowBox[{"Dimensionless", " ", "Linearized", " ", "Hip", " ", "Position"}],
     "*)"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   RowBox[{"(*", "COM", "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["xp", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt", "+", "mf"}], ")"}]}], "+", "mh"}], 
             ")"}]}]}]}], "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", 
      "Simplify"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["zp", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt", "+", "mf"}], ")"}]}], "+", "mh"}], 
             ")"}]}]}]}], "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", 
      "Simplify"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Pcom", " ", "=", " ", 
     SubscriptBox["xp", "COM"]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[Delta]Pcom", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"Pcom", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stcomslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["xp", "COM"]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["zp", "COM"]}]], "/.", "p0"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[Delta]stcomslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"stcomslope", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"nstcomslope", " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["xp", "COM"]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["zp", "COM"]}]], "/.", "p0"}], "/.", "constsubs"}], "//",
      "Simplify"}]}], "\n", 
   RowBox[{
    RowBox[{"\[Delta]nstcomslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstcomslope", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "COM"], "=", 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           UnderoverscriptBox["\[Sum]", 
            RowBox[{"i", "=", "1"}], 
            RowBox[{"Length", "[", "mm", "]"}]], 
           RowBox[{
            RowBox[{
            "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
            RowBox[{
             RowBox[{
              RowBox[{
               SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
              "\[LeftDoubleBracket]", 
              RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"2", "*", 
                RowBox[{"(", 
                 RowBox[{"mc", "+", "mt", "+", "mf"}], ")"}]}], "+", "mh"}], 
              ")"}]}]}]}], "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", 
       "Simplify"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Linearized", " ", "COM"}], "*)"}], "\[IndentingNewLine]", 
     RowBox[{
      SubscriptBox["Linearp", "COM"], "=", " ", 
      RowBox[{"Linearize", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{
           RowBox[{"Subscript", "[", 
            RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
    "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527341306783598*^9, 
   3.5273413258006372`*^9}, {3.5273418861526527`*^9, 3.527341944516119*^9}, 
   3.527342084297084*^9, {3.527342119801113*^9, 3.527342122074991*^9}, 
   3.5273422671086893`*^9, 3.5273434152081633`*^9, {3.527343919855567*^9, 
   3.527343961917466*^9}, {3.529328352989338*^9, 3.529328362792905*^9}, 
   3.5293284284450502`*^9, {3.5293284812437*^9, 3.529328559899397*^9}, 
   3.5293285906250753`*^9, {3.529330020956822*^9, 3.5293300258861437`*^9}, {
   3.529330194308785*^9, 3.529330222592731*^9}, {3.529330254897019*^9, 
   3.529330282163104*^9}, {3.529330486601256*^9, 3.529330499020458*^9}, 
   3.529330879783185*^9, 3.529342665423951*^9, {3.5293427019606667`*^9, 
   3.5293427319337883`*^9}, {3.529355700592792*^9, 3.529355701244479*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"-", 
     RowBox[{"457593477", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
    RowBox[{"1637194622", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
    RowBox[{"14065902578", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
    RowBox[{"12759122123", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
   RowBox[{
    RowBox[{"457593477", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
    RowBox[{"1637194622", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
    RowBox[{"14065902578", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
    RowBox[{"12759122123", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}]], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529342666199264*^9, {3.5293427071188803`*^9, 3.5293427327230453`*^9}, 
   3.529356364059786*^9, 3.5293607140416803`*^9, 3.5293617213402967`*^9, 
   3.5293618292065153`*^9, 3.529429903905209*^9, 3.5294300327674303`*^9, 
   3.529581335706213*^9, 3.529584912267769*^9, 3.529586473588872*^9, 
   3.529589182404667*^9, 3.529616555235409*^9, 3.529620621004711*^9, 
   3.529622175588705*^9, 3.529711940501492*^9, 3.5297553597417183`*^9, 
   3.529762993671946*^9, 3.5297666815300417`*^9, 3.529768068284*^9, 
   3.529768533497188*^9, 3.5297950426592503`*^9, 3.530716105642523*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["xp", "nsf"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "constsubs"}],
     "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["zp", "nsf"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
      "constsubs"}], "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"length", " ", "between", " ", "COM", " ", "and", " ", "non"}], 
    "-", 
    RowBox[{"stance", " ", "foot"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"nstCOMLen", " ", "=", " ", 
    RowBox[{"Sqrt", "[", 
     RowBox[{
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["xp", "COM"], "-", 
         SubscriptBox["xp", "nsf"]}], ")"}], "2"], "+", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["zp", "COM"], "-", 
         SubscriptBox["zp", "nsf"]}], ")"}], "2"]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"Linearized", " ", "stCOMLen"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"\[Delta]nstCOMLen", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{
     RowBox[{"stCOMLen", "/.", "p0"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"Subscript", "[", 
         RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["xp", "stf"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "constsubs"}],
     "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["zp", "stf"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
      "constsubs"}], "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "length", " ", "between", " ", "COM", " ", "and", " ", "stance", " ", 
    "foot"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"stCOMLen", " ", "=", " ", 
    RowBox[{"Sqrt", "[", 
     RowBox[{
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["xp", "COM"], "-", 
         SubscriptBox["xp", "stf"]}], ")"}], "2"], "+", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["zp", "COM"], "-", 
         SubscriptBox["zp", "stf"]}], ")"}], "2"]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"Linearized", " ", "stCOMLen"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"\[Delta]stCOMLen", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"stCOMLen", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "*)"}]}]}], "Input",
 CellChangeTimes->{{3.5293259137171497`*^9, 3.529325966911764*^9}, {
  3.529326216593337*^9, 3.529326217432181*^9}, {3.529326575054517*^9, 
  3.5293266322949657`*^9}, {3.529326960111162*^9, 3.529326971619865*^9}, {
  3.529405999848874*^9, 3.529406003352455*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"COM", " ", "Angle"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{"COMAngle", " ", "=", " ", 
    RowBox[{
     RowBox[{"ArcTan", "[", " ", 
      RowBox[{
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["xp", "COM"]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["zp", "COM"]}]], "/.", "p0"}], "]"}], " ", "-", 
     RowBox[{"ArcTan", "[", " ", 
      RowBox[{
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["xp", "COM"]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["zp", "COM"]}]], "/.", "p0"}], "]"}]}]}], 
   ";"}]}]], "Input",
 CellChangeTimes->{{3.529753849438113*^9, 3.529753854349491*^9}, {
   3.529754076827833*^9, 3.5297541231978283`*^9}, {3.529754204619493*^9, 
   3.529754213479525*^9}, 3.529755230468444*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"feedback", " ", "control"}], "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Hip", " ", "Position", " ", 
      RowBox[{"Output", ":", 
       RowBox[{"1.", " ", "hip", " ", "position"}]}]}], ";", 
     RowBox[{
     "2.", " ", "Linearized", " ", "hip", " ", "position", " ", "3.", " ", 
      "Dimensionless", " ", "Linearized", " ", "hip", " ", "position"}]}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"Pcom", ",", "stcomslope", ",", "\[Delta]Pcom"}], "}"}]}], ";", 
    
    RowBox[{"HipPosIndex", "=", "1"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "1", "]"}], "]"}]}], " ", ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"time", "-", 
      RowBox[{"invariant", " ", 
       RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"calculate", 
      FormBox[
       SubscriptBox["y", "d"],
       TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], 
    "\n", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", "and",
      " ", "Jacobians"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "nstcomslope", " ", ",", "COMAngle", ",", "\[Delta]nstcomslope", " ", 
       ",", "nsslope", ",", "HipAngle"}], "}"}]}], ";"}], 
   "\[IndentingNewLine]", " ", "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"TorsoOutput", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
       "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
        ",", "storso", ",", "LinearStorso", ",", 
        SubscriptBox["p", "COM"], ",", 
        SubscriptBox["Linearp", "COM"]}], "}"}]}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "1", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "}"}]}], "}"}], "/.",
         "constsubs"}], "/.", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"NSslopeOutput", "[", 
             RowBox[{"[", "1", "]"}], "]"}], "}"}], ",", 
           RowBox[{"{", "stCOMLen", "}"}], ",", 
           RowBox[{"{", "nstCOMLen", "}"}]}], "}"}], "/.", "constsubs"}], "/.",
         "p0"}], "]"}]}], ";"}], " ", 
    RowBox[{"(*", "nstCOMLen", "*)"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
    ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527341306783598*^9, 
   3.5273413258006372`*^9}, {3.5273418861526527`*^9, 3.527341944516119*^9}, 
   3.527342084297084*^9, {3.527342119801113*^9, 3.527342122074991*^9}, 
   3.5273422671086893`*^9, 3.5273434152081633`*^9, {3.527343919855567*^9, 
   3.527343961917466*^9}, {3.529328352989338*^9, 3.529328362792905*^9}, 
   3.5293284284450502`*^9, {3.5293284812437*^9, 3.529328559899397*^9}, 
   3.5293285906250753`*^9, {3.529330020956822*^9, 3.5293300258861437`*^9}, {
   3.529330194308785*^9, 3.529330222592731*^9}, {3.529330254897019*^9, 
   3.529330282163104*^9}, {3.5293305128316927`*^9, 3.529330530357297*^9}, {
   3.5293305658957853`*^9, 3.529330594084263*^9}, {3.529330761511338*^9, 
   3.52933077058587*^9}, {3.529330823590879*^9, 3.529330841582114*^9}, 
   3.529330895108952*^9, {3.529333478177815*^9, 3.529333478324348*^9}, {
   3.5293562839409513`*^9, 3.529356310112143*^9}, {3.52936074403654*^9, 
   3.529360744283927*^9}, {3.529429853085973*^9, 3.52942989547523*^9}, {
   3.529430014897098*^9, 3.5294300231233263`*^9}, {3.5295813099184732`*^9, 
   3.5295813279741907`*^9}, {3.529584900442463*^9, 3.529584904041497*^9}, {
   3.5295864637819147`*^9, 3.529586465270554*^9}, {3.5295891719300623`*^9, 
   3.52958917208008*^9}, {3.529616524063208*^9, 3.529616541715241*^9}, {
   3.529620588870631*^9, 3.52962060907419*^9}, {3.529622109184564*^9, 
   3.5296221398145323`*^9}, {3.5297118863477783`*^9, 3.5297119248892*^9}, {
   3.529755249921115*^9, 3.529755257239594*^9}, {3.529762958439692*^9, 
   3.52976296595061*^9}, {3.5297666642684917`*^9, 3.529766672197433*^9}, {
   3.529767790769846*^9, 3.5297678004194736`*^9}, {3.529768500325581*^9, 
   3.529768518741641*^9}, {3.529795005073854*^9, 3.52979503224088*^9}, {
   3.5307160866202908`*^9, 3.5307160967399683`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 SubscriptBox["y", 
  RowBox[{"a", ",", "2"}]]], "Input"],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "457593477"}], " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"1637194622", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"14065902578", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"12759122123", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       RowBox[{
        RowBox[{"457593477", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"1637194622", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"14065902578", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"12759122123", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}]]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.529333468667893*^9, 3.529333530208696*^9, 
  3.529356410065867*^9, 3.529360733087666*^9, 3.5293617479923153`*^9, 
  3.5293618554029713`*^9, 3.529429938151484*^9, 3.5294302683536873`*^9, 
  3.529581597405066*^9, 3.529585166245426*^9, 3.529586507171947*^9, 
  3.529589422199293*^9, 3.5296165806039553`*^9, 3.5296208491821117`*^9, 
  3.529622203435128*^9, 3.5297119743692102`*^9, 3.529755422286696*^9, 
  3.529763019782674*^9, 3.529766743206997*^9, 3.529768094983857*^9, 
  3.529768568224038*^9, 3.529795043324739*^9, 3.530716132639189*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"\n", 
  RowBox[{"(*", 
   RowBox[{"zero", " ", "dynamics"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Zstatesubs", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"z", ",", " ", "1"}], "]"}], "[", "t", "]"}], " ", "->", 
        " ", 
        FormBox[
         RowBox[{"z", "[", "1", "]"}],
         TraditionalForm]}], ",", " ", "\n", "  ", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"z", ",", " ", "2"}], "]"}], "[", "t", "]"}], " ", "->", 
        " ", 
        FormBox[
         RowBox[{"z", "[", "2", "]"}],
         TraditionalForm]}]}], "}"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"c", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"Flatten", "[", 
         SubscriptBox["p", "hip"], "]"}]}], " ", "/.", " ", "constsubs"}], 
      "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"H", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"Flatten", "[", 
         SubscriptBox["y", 
          RowBox[{"a", ",", "2"}]], "]"}]}], " ", "/.", " ", "constsubs"}], 
      "]"}]}], ";", "\n", 
    RowBox[{"Phi", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"fbz", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
            RowBox[{"p", "[", "1", "]"}]}], ")"}], "/", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}]}], "}"}], "\n", 
       RowBox[{"Subscript", "[", 
        RowBox[{"yz", ",", "d", ",", "2"}], "]"}]}], "=", 
      RowBox[{
       RowBox[{"Transpose", "[", 
        RowBox[{"{", 
         RowBox[{"Table", "[", 
          RowBox[{
           RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
           RowBox[{"{", 
            RowBox[{"i", ",", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
        "]"}], "/.", 
       RowBox[{"fbz", "\n", 
        RowBox[{"PhiI", "=", 
         RowBox[{"Simplify", "[", 
          RowBox[{
           RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
           RowBox[{"Join", "[", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{"{", 
               RowBox[{
                RowBox[{"Subscript", "[", 
                 RowBox[{"z", ",", "1"}], "]"}], "[", "t", "]"}], "}"}], 
              "}"}], ",", 
             RowBox[{"Subscript", "[", 
              RowBox[{"yz", ",", "d", ",", "2"}], "]"}]}], "]"}]}], 
          "]"}]}]}]}]}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"PhidotI", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Subscript", "[", 
             RowBox[{"z", ",", "2"}], "]"}], "[", "t", "]"}], "}"}], "}"}], 
         ",", 
         RowBox[{
          RowBox[{"D", "[", 
           RowBox[{
            RowBox[{"Subscript", "[", 
             RowBox[{"yz", ",", "d", ",", "2"}], "]"}], ",", 
            RowBox[{
             RowBox[{"Subscript", "[", 
              RowBox[{"z", ",", "1"}], "]"}], "[", "t", "]"}]}], "]"}], "*", 
          RowBox[{
           RowBox[{"Subscript", "[", 
            RowBox[{"z", ",", "2"}], "]"}], "[", "t", "]"}]}]}], "]"}]}], 
      "]"}]}], ";"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "write", " ", "the", " ", "control", " ", "files", " ", "to", " ", 
    "disk"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_ntorso\>\""}],
      "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{"hdot", "/.", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{"\[Sigma]y", "/.", "constsubs"}], "/.", "statesubs"}]}], "]"}],
     ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "hipdot"], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "hip"], "/.", "constsubs"}], "/.", "statesubs"}]}], 
     "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"stream", "=", 
      RowBox[{"OpenWrite", "[", "\"\<pcom_sca\>\"", "]"}]}], ";", "\n", 
     RowBox[{"Write", "[", 
      RowBox[{"stream", ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "COM"], "/.", "constsubs"}], "/.", "statesubs"}]}],
       "]"}], ";", "\n", 
     RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
     RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"d", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"d", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"a", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"a", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"a", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"a", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527341306783598*^9, 
   3.5273413258006372`*^9}, {3.5273418861526527`*^9, 3.527341944516119*^9}, 
   3.527342084297084*^9, {3.527342119801113*^9, 3.527342122074991*^9}, 
   3.5273422671086893`*^9, 3.5273434152081633`*^9, {3.527343919855567*^9, 
   3.527343961917466*^9}, {3.529328352989338*^9, 3.529328362792905*^9}, 
   3.5293284284450502`*^9, {3.5293284812437*^9, 3.529328559899397*^9}, 
   3.5293285906250753`*^9, {3.529330020956822*^9, 3.5293300258861437`*^9}, {
   3.529330194308785*^9, 3.529330222592731*^9}, {3.529330254897019*^9, 
   3.529330282163104*^9}, {3.5293305128316927`*^9, 3.529330530357297*^9}, {
   3.5293305658957853`*^9, 3.529330594084263*^9}, {3.529330761511338*^9, 
   3.52933077058587*^9}, {3.529330823590879*^9, 3.529330841582114*^9}, 
   3.529330895108952*^9, {3.529330953018791*^9, 3.529331007897126*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "perl", "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"LineNumber", " ", "=", "2"}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"Run", "[", "\"\<perl math2mat_ntorso.pl\>\"", "]"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "SetDirectory", "[", 
      "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs/build_torso\>\"",
       "]"}], ";"}], "*)"}], "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527341306783598*^9, 
   3.5273413258006372`*^9}, {3.5273418861526527`*^9, 3.527341944516119*^9}, 
   3.527342084297084*^9, {3.527342119801113*^9, 3.527342122074991*^9}, 
   3.5273422671086893`*^9, 3.5273434152081633`*^9, {3.527343919855567*^9, 
   3.527343961917466*^9}, {3.529328352989338*^9, 3.529328362792905*^9}, 
   3.5293284284450502`*^9, {3.5293284812437*^9, 3.529328559899397*^9}, 
   3.5293285906250753`*^9, {3.529330020956822*^9, 3.5293300258861437`*^9}, {
   3.529330194308785*^9, 3.529330222592731*^9}, {3.529330254897019*^9, 
   3.529330282163104*^9}, {3.5293305128316927`*^9, 3.529330530357297*^9}, {
   3.529331044276582*^9, 3.529331067185398*^9}, 3.529331208467557*^9}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529331208821986*^9, 3.529333537252493*^9, 3.5293564722190933`*^9, 
   3.529360733335052*^9, 3.52936174824162*^9, 3.529361855631526*^9, 
   3.529429938464514*^9, 3.5294302689644814`*^9, 3.529430368441957*^9, 
   3.529581598124423*^9, 3.529585166888834*^9, 3.5295865074895144`*^9, {
   3.529587553516877*^9, 3.529587568603643*^9}, 3.5295894227759323`*^9, 
   3.529616580949177*^9, 3.5296208497546177`*^9, 3.529622203685533*^9, 
   3.529711974694058*^9, 3.529755422576593*^9, {3.529755544207635*^9, 
   3.529755558848833*^9}, 3.5297556194944563`*^9, 3.529755686428632*^9, 
   3.5297630201038322`*^9, 3.529766743509697*^9, 3.529768095294217*^9, 
   3.5297685685251217`*^9, 3.5297950435206337`*^9, 3.530716132863832*^9}]
}, Open  ]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.5158840455366793`*^9, 3.515884055622256*^9}, {
   3.5158841551359477`*^9, 3.5158841624563665`*^9}, {3.515891350156479*^9, 
   3.515891350585504*^9}, 3.529340957221448*^9}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"initialtime", "=", 
   RowBox[{"{", 
    RowBox[{"t", "\[Rule]", "0"}], "}"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"\[CapitalDelta]", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"1", ",", "1", ",", "1", ",", 
       RowBox[{"-", "1"}]}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", " ", "0", ",", " ", "0", ",", "1"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", 
       RowBox[{"-", "1"}], ",", " ", "0"}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", " ", "1", ",", " ", "0", ",", " ", "0"}], "}"}]}], 
    "}"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"resetmap", "=", 
  RowBox[{"Module", "[", 
   RowBox[{
    RowBox[{"{", "newq", "}"}], ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"newq", "=", 
      RowBox[{"\[CapitalDelta]", ".", 
       RowBox[{"(", "q", ")"}]}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "0", "]"}], "->", 
        RowBox[{"newq", "[", 
         RowBox[{"[", 
          RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", 
         RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}]}]}], 
   "\[IndentingNewLine]", "]"}]}]}], "Input",
 CellChangeTimes->{{3.5126870727453375`*^9, 3.5126871090532956`*^9}, {
   3.512687198656415*^9, 3.5126872022367573`*^9}, {3.5126906830481005`*^9, 
   3.5126907051903095`*^9}, {3.514054842423344*^9, 3.514054877019777*^9}, 
   3.514054909726557*^9, {3.5158588592522273`*^9, 3.5158589038437777`*^9}, {
   3.5158839903865247`*^9, 3.5158839971019087`*^9}, {3.529340664938408*^9, 
   3.529340678688675*^9}, {3.529340958799315*^9, 3.529340959987949*^9}, 
   3.529361820828527*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "(", "0", ")"}], "\[Rule]", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "0", ")"}], "\[Rule]", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "0", ")"}], "\[Rule]", 
     RowBox[{"-", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "0", ")"}], "\[Rule]", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}]}], "}"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5293406863863297`*^9, {3.529340952902034*^9, 3.529340960370401*^9}, 
   3.52935648687284*^9, 3.529360733400769*^9, 3.529361748307673*^9, 
   3.529361855675214*^9, 3.5294299385049267`*^9, 3.529430269042941*^9, 
   3.5295815982058287`*^9, 3.5295851669843616`*^9, 3.529586507532906*^9, 
   3.529589422811603*^9, 3.529616581047513*^9, 3.529620849826647*^9, 
   3.529622203742465*^9, 3.529711974812151*^9, 3.529755422649762*^9, 
   3.5297630201959963`*^9, 3.529766743598153*^9, 3.529768095393464*^9, 
   3.529768568619896*^9, 3.529795043597945*^9, 3.530716132915473*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"phipcondition", "=", 
   RowBox[{
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"p", "[", "1", "]"}], "->", 
       SubscriptBox["p", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
    RowBox[{"t", "\[Rule]", "0"}]}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"tau", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
        "fb"}], "/.", "\[InvisibleSpace]", "phipcondition"}], "/.", 
      "resetmap"}], "/.", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["y", "2"], "=", " ", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "  ", "-", " ", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]]}], "  ", ")"}], "/.", "\[InvisibleSpace]", 
    "phipcondition"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"yplus", "=", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["y", "2"], "/.", "initialtime"}], "/.", "constsubs"}], "/.",
     "resetmap"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"yminus", "=", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", "2"], "/.", "constsubs"}], "/.", "resetmap"}]}], 
   ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"Y", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"{", 
       SubscriptBox["p", "hip"], "}"}]}], ",", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["y", "2"], "]"}]}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Yplus", "=", 
   RowBox[{
    RowBox[{
     RowBox[{"Y", "/.", "initialtime"}], "/.", "constsubs"}], "/.", 
    "resetmap"}]}], ";"}], "\n", 
 RowBox[{"Yminus", "=", 
  RowBox[{
   RowBox[{"Y", "/.", "constsubs"}], "/.", "resetmap"}]}], "\n", 
 RowBox[{
  RowBox[{"Dh", " ", "=", " ", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"{", "h", "}"}]}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.515889672571527*^9, 3.515889674968664*^9}, {
   3.515890444251664*^9, 3.515890444569683*^9}, {3.529340743183091*^9, 
   3.5293407452691174`*^9}, {3.529340970527377*^9, 3.5293409767689333`*^9}, {
   3.529341285728715*^9, 3.529341291472875*^9}, {3.529341835566811*^9, 
   3.529341855545938*^9}, {3.5293419715798473`*^9, 3.5293419784195633`*^9}, 
   3.529586381230159*^9}],

Cell[BoxData[
 FormBox[
  InterpretationBox[
   TagBox[
    PanelBox[GridBox[{
       {
        StyleBox[
         StyleBox[
          DynamicBox[ToBoxes[
            FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"], 
            TraditionalForm],
           ImageSizeCache->{542., {4., 14.}}],
          StripOnInput->False,
          DynamicUpdating->True], "Panel",
         StripOnInput->False,
         Background->None]},
       {
        ItemBox[
         TagBox[
          RowBox[{"(", "\[NoBreak]", 
           RowBox[{"\[LeftSkeleton]", "1", "\[RightSkeleton]"}], "\[NoBreak]",
            ")"}],
          Short[#, 5]& ],
         Background->GrayLevel[1],
         BaseStyle->{Deployed -> False},
         Frame->True,
         FrameStyle->GrayLevel[0, 0.2],
         StripOnInput->False]},
       {
        RowBox[{
         ButtonBox[
          StyleBox[
           StyleBox[
            DynamicBox[ToBoxes[
              FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], 
              TraditionalForm],
             ImageSizeCache->{95., {0., 14.}}],
            StripOnInput->False,
            DynamicUpdating->True], "Panel",
           StripOnInput->False,
           Background->None],
          Appearance->Automatic,
          
          ButtonFunction:>OutputSizeLimit`ButtonFunction[
           Identity, 207, 23138759242093245696, 5/2],
          Enabled->True,
          Evaluator->Automatic,
          Method->"Queued"], "\[ThinSpace]", 
         ButtonBox[
          StyleBox[
           StyleBox[
            DynamicBox[ToBoxes[
              FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], 
              TraditionalForm],
             ImageSizeCache->{99., {0., 14.}}],
            StripOnInput->False,
            DynamicUpdating->True], "Panel",
           StripOnInput->False,
           Background->None],
          Appearance->Automatic,
          
          ButtonFunction:>OutputSizeLimit`ButtonFunction[
           Identity, 207, 23138759242093245696, 5 2],
          Enabled->True,
          Evaluator->Automatic,
          Method->"Queued"], "\[ThinSpace]", 
         ButtonBox[
          StyleBox[
           StyleBox[
            DynamicBox[ToBoxes[
              FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], 
              TraditionalForm],
             ImageSizeCache->{156., {4., 14.}}],
            StripOnInput->False,
            DynamicUpdating->True], "Panel",
           StripOnInput->False,
           Background->None],
          Appearance->Automatic,
          
          ButtonFunction:>OutputSizeLimit`ButtonFunction[
           Identity, 207, 23138759242093245696, Infinity],
          Enabled->True,
          Evaluator->Automatic,
          Method->"Queued"], "\[ThinSpace]", 
         ButtonBox[
          StyleBox[
           StyleBox[
            DynamicBox[ToBoxes[
              FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], 
              TraditionalForm],
             ImageSizeCache->{143., {0., 14.}}],
            StripOnInput->False,
            DynamicUpdating->True], "Panel",
           StripOnInput->False,
           Background->None],
          Appearance->Automatic,
          ButtonFunction:>FrontEndExecute[{
             FrontEnd`SetOptions[
             FrontEnd`$FrontEnd, 
              FrontEnd`PreferencesSettings -> {"Page" -> "Evaluation"}], 
             FrontEnd`FrontEndToken["PreferencesDialog"]}],
          Evaluator->None,
          Method->"Preemptive"]}]}
      },
      GridBoxAlignment->{
       "Columns" -> {{Left}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
        "RowsIndexed" -> {}},
      GridBoxDividers->{
       "Columns" -> {{False}}, "ColumnsIndexed" -> {}, "Rows" -> {{False}}, 
        "RowsIndexed" -> {}},
      GridBoxItemSize->{
       "Columns" -> {{Automatic}}, "ColumnsIndexed" -> {}, "Rows" -> {{1.}}, 
        "RowsIndexed" -> {}},
      GridBoxSpacings->{"Columns" -> {
          Offset[0.27999999999999997`], {
           Offset[0.5599999999999999]}, 
          Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
          Offset[0.2], {
           Offset[1.2]}, 
          Offset[0.2]}, "RowsIndexed" -> {}}],
     DefaultBaseStyle->{},
     FrameMargins->5],
    Deploy,
    DefaultBaseStyle->{Deployed -> True}],
   Out[207]], TraditionalForm]], "Output",
 CellChangeTimes->{3.529586382664323*^9, 3.529586507824074*^9, 
  3.529589423455987*^9, 3.5296165813117237`*^9, 3.5296208504899054`*^9, 
  3.529622203981551*^9, 3.529711975108677*^9, 3.5297554229781637`*^9, 
  3.529763020442298*^9, 3.529766743903763*^9, 3.529768095674739*^9, 
  3.529768568952574*^9, 3.5297950436954813`*^9, 3.530716133195909*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", 
    "\"\<buildopt_ntorso\>\""}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<tau\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"tau", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"Yplus", "/.", "statesubs"}], "/.", "constsubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 StyleBox[
  RowBox[{
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<h_dot_minus\>\"", "]"}]}], ";"}],
  FontWeight->"Bold"], "\n", 
 StyleBox[
  RowBox[{
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"Dh", "/.", "statesubs"}]}], "]"}], ";"}],
  FontWeight->"Bold"], "\n", 
 StyleBox[
  RowBox[{
   RowBox[{"Close", "[", "stream", "]"}], ";"}],
  FontWeight->"Bold"], "\n", 
 StyleBox[
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}],
  FontWeight->"Bold"]}], "Input",
 CellChangeTimes->{{3.529340754629857*^9, 3.529340754638857*^9}, 
   3.529341068321127*^9, {3.5293412378921824`*^9, 3.529341249063349*^9}, {
   3.529342264116734*^9, 3.529342270195084*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}], "\n", 
 RowBox[{"Run", "[", "\"\<perl math2matopt_ntorso.pl\>\"", "]"}]}], "Input",
 CellChangeTimes->{{3.52934094557617*^9, 3.529340945578516*^9}, {
  3.529341257558341*^9, 3.52934125908011*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{3.529341863619225*^9, 3.52934230933177*^9, 
  3.529356495020347*^9, 3.529360733660716*^9, 3.529361748662237*^9, 
  3.529361856033079*^9, 3.52942993882055*^9, 3.529430269687187*^9, 
  3.529430371867053*^9, 3.529581598949645*^9, 3.529585167765779*^9, 
  3.52958650796872*^9, 3.529589423632633*^9, 3.529616581467752*^9, 
  3.529620850669589*^9, 3.529622204096109*^9, 3.5297119754481792`*^9, 
  3.5297554231654577`*^9, 3.5297557014032507`*^9, 3.529763020605671*^9, 
  3.529766744113969*^9, 3.5297680958359623`*^9, 3.529768569230798*^9, 
  3.529795043940818*^9, 3.5307161334729233`*^9}]
}, Open  ]]
},
WindowSize->{1920, 1026},
WindowMargins->{{-9, Automatic}, {Automatic, -8}},
ShowSelection->True,
Magnification:>FEPrivate`If[
  FEPrivate`Equal[FEPrivate`$VersionNumber, 6.], 1.25, 1.25 Inherited],
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 2328, 39, 152, "Input"],
Cell[2898, 63, 550, 9, 38, "Output"]
}, Open  ]],
Cell[3463, 75, 36309, 1059, 2061, "Input"],
Cell[CellGroupData[{
Cell[39797, 1138, 16558, 447, 989, "Input"],
Cell[56358, 1587, 2904, 77, 68, "Output"]
}, Open  ]],
Cell[59277, 1667, 4234, 131, 304, "Input"],
Cell[63514, 1800, 1609, 44, 100, "Input"],
Cell[65126, 1846, 12854, 344, 790, "Input"],
Cell[CellGroupData[{
Cell[78005, 2194, 71, 2, 36, "Input"],
Cell[78079, 2198, 3757, 95, 122, "Output"]
}, Open  ]],
Cell[81851, 2296, 17551, 487, 2533, "Input"],
Cell[CellGroupData[{
Cell[99427, 2787, 2661, 47, 152, "Input"],
Cell[102091, 2836, 796, 12, 53, "Output"]
}, Open  ]],
Cell[102902, 2851, 220, 3, 36, "Input"],
Cell[CellGroupData[{
Cell[103147, 2858, 1911, 47, 221, "Input"],
Cell[105061, 2907, 1621, 41, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[106719, 2953, 3121, 95, 316, "Input"],
Cell[109843, 3050, 4707, 129, 159, "Output"]
}, Open  ]],
Cell[114565, 3182, 2783, 84, 589, "Input"],
Cell[CellGroupData[{
Cell[117373, 3270, 311, 6, 60, "Input"],
Cell[117687, 3278, 657, 10, 53, "Output"]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
